#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

// 创建当前状态对象，并将回调函数state_cb返回的数据赋值到其中
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

// 创建的无人机当前位置对象，并将回调函数local_pos_cb返回的数据赋值到其中
geometry_msgs::PoseStamped local_pos;
void local_pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    local_pos = *msg;
}

// 创建期望位置对象
geometry_msgs::PoseStamped expect_pos;

// 无人机位置控制函数
// 返回值： 无人机速度信号
// 输入参数： local_postion 	当前位置 
// 输入参数： expect_postion 期望位置
geometry_msgs::Twist position_contrl_velocity(geometry_msgs::PoseStamped local_postion,geometry_msgs::PoseStamped expect_postion)
{
    // 创建输出速度信号对象
    geometry_msgs::Twist velocity_cmd; 

    // 输出线速度信号为 期望位置 - 当前位置
    // 当期望位置与当前位置误差为0时，则说明到达期望位置，速度为0，无人机静止
    velocity_cmd.linear.x = (expect_postion.pose.position.x - local_postion.pose.position.x);
    velocity_cmd.linear.y = (expect_postion.pose.position.y - local_postion.pose.position.y);
    velocity_cmd.linear.z = (expect_postion.pose.position.z - local_postion.pose.position.z);
    //角速度用不上，均为0
    velocity_cmd.angular.x = 0.0;
    velocity_cmd.angular.y = 0.0;
    velocity_cmd.angular.z = 0.0;
    // 线速度限幅为2m/s
    if(velocity_cmd.linear.x > 2.0)
    {
        velocity_cmd.linear.x = 2.0;
    }
    else if(velocity_cmd.linear.x < -2.0)
    {
        velocity_cmd.linear.x = -2.0;
    }
    if(velocity_cmd.linear.y > 2.0)
    {
        velocity_cmd.linear.y = 2.0;
    }
    else if(velocity_cmd.linear.y < -2.0)
    {
        velocity_cmd.linear.y = -2.0;
    }
    if(velocity_cmd.linear.z > 2.0)
    {
        velocity_cmd.linear.z = 2.0;
    }
    else if(velocity_cmd.linear.z < -2.0)
    {
        velocity_cmd.linear.z = -2.0;
    }
    // 返回速度信号
    return velocity_cmd;
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "offboard_node_cluster");
    ros::NodeHandle nh;

    // 订阅无人机状态话题
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("iris_0/mavros/state", 10, state_cb);

    // 订阅无人机当前位置话题
    ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>
            ("iris_0/mavros/local_position/pose", 10, local_pos_cb);

    // 发布无人机速度控制消息
    ros::Publisher local_velocity_pub = nh.advertise<geometry_msgs::Twist>
            ("iris_0/mavros/setpoint_velocity/cmd_vel_unstamped", 10);
    
    //the setpoint publishing rate MUST be faster than 2Hz
    //这是为了保证无人机能正常切换到offboard模式（控制信号发送频率必须大于2Hz）
    ros::Rate rate(20.0);

    // wait for FCU connection
    // 等待无人机连接成功
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    //设置初始期望位置
    expect_pos.pose.position.x = 0;
    expect_pos.pose.position.y = 0;
    expect_pos.pose.position.z = 3;

    //创建速度信号对象，并初始化
    geometry_msgs::Twist velocity;
    velocity.linear.x = 0.0;
    velocity.linear.y = 0.0;
    velocity.linear.z = 0.0;
    velocity.angular.x = 0.0;
    velocity.angular.y = 0.0;  
    velocity.angular.z = 0.0;

    //send a few setpoints before starting
    //发送足量的控制信号，用以保证切换offboard模式成功
    for(int i = 100; ros::ok() && i > 0; --i)
    {
        //以固定频率发布速度消息
        local_velocity_pub.publish(velocity);
        ros::spinOnce();
        rate.sleep();
    }

    // 设置预定路径
    float circle_radius = 10.0; // 设置圆的半径
    float circle_center_x = 0.0; // 设置圆心的x坐标
    float circle_center_y = 0.0; // 设置圆心的y坐标
    float theta = 0.0; // 角度

    // 等无人机解锁后再进行轨迹跟踪
    while(ros::ok() && !current_state.armed)
    {
        local_velocity_pub.publish(velocity);
        ROS_INFO("wait UAV arm");
        ros::spinOnce();
        rate.sleep();
    }
    ROS_INFO("UAV armed");
    while(ros::ok())
    {
        expect_pos.pose.position.x = 0;
        expect_pos.pose.position.y = 0;
        expect_pos.pose.position.z = 1.5; // 设定期望的高度

        // 发送速度控制消息
        local_velocity_pub.publish(position_contrl_velocity(local_pos,expect_pos));
        // 打印当前无人机位置
        ROS_INFO("local_postion: x[%.2f] y[%.2f] z[%.2f]",local_pos.pose.position.x,local_pos.pose.position.y,local_pos.pose.position.z);
        
        if(local_pos.pose.position.z > 1.5)
        {
            ROS_INFO("local_postion is expect_pos");
        }

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

